#!/usr/bin/python

import rospy
from geometry_msgs.msg import Twist,Point
from nav_msgs.msg import Odometry
import math

current_position = Point()
target_distance = 0.5
min_velocity = 0.01
max_velocity = 0.03

def get_odom(data) :
    global current_position 
    current_position = data.pose.pose.position

def cmd_vel_pub():
    global target_distance
    rospy.init_node('line_loop',anonymous=False)
    cmd_pub = rospy.Publisher ('/cmd_vel', Twist, queue_size=10)
    odom_sub = rospy.Subscriber("odom" , Odometry, get_odom)
    rate = rospy.Rate(10)
    twist = Twist()
    rospy.loginfo('Start Control Robot Line Loop')
    forword_flag = True
    while not rospy.is_shutdown():
        start_position = current_position
        while True:   
            error = target_distance - ((current_position.x - start_position.x)**2 +(current_position.y - start_position.y)**2)**0.5
            if(error > 0):
                twist.linear.x = math.sin(error*3.14)

                if twist.linear.x < min_velocity:
                    twist.linear.x = min_velocity
                if twist.linear.x > max_velocity:
                    twist.linear.x = max_velocity
                if not forword_flag:
                    twist.linear.x*= -1.0
                else:
                    pass
                cmd_pub.publish(twist)
            else:
                twist.linear.x = 0.0
                cmd_pub.publish(twist)
                break
            rate.sleep()
        forword_flag = bool(1-forword_flag)

if __name__ == '__main__':
    try:
        cmd_vel_pub()
    except:
        rospy.loginfo("line loop terminated.")
